/*
 *	This file is part of qp42.
 *
 *	qp42 -- An Implementation of qp solver on 42 intervals.
 *	Copyright (C) 2012 by Janick Frasch, Hans Joachim Ferreau et al. 
 *	All rights reserved.
 *
 *	qp42 is free software; you can redistribute it and/or
 *	modify it under the terms of the GNU Lesser General Public
 *	License as published by the Free Software Foundation; either
 *	version 2.1 of the License, or (at your option) any later version.
 *
 *	qp5 is distributed in the hope that it will be useful,
 *	but WITHOUT ANY WARRANTY; without even the implied warranty of
 *	MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
 *	See the GNU Lesser General Public License for more details.
 *
 *	You should have received a copy of the GNU Lesser General Public
 *	License along with qp42; if not, write to the Free Software
 *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
 *
 */


/**
 *	\file examples/wang2010.c
 *	\author Janick Frasch, Hans Joachim Ferreau
 *	\version 1.0beta
 *	\date 2012
 *
 *	Example from Wang2010. Matrices generated using setupOscillator.m (in matlab examples)
 */



#include <qpDUNES.h>

#define INFTY 1.0e12

int main( )
{
	int i;
	int k;
	boolean_t isLTI;
	
	unsigned int nI = 30;
	unsigned int nX = 12;
	unsigned int nU = 3;
	unsigned int* nD = 0;
//	unsigned int nD[30+1] =
//		{	12+3, 12+3, 12+3, 12+3, 12+3, 12+3, 12+3, 12+3, 12+3, 12+3,	/* 10 */
//			12+3, 12+3, 12+3, 12+3, 12+3, 12+3, 12+3, 12+3, 12+3, 12+3,	/* 10 */
//			12+3, 12+3, 12+3, 12+3, 12+3, 12+3, 12+3, 12+3, 12+3, 12+3,	/* 10 */
//			12
//		};

		
	double x0[12] = 
		{ 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0 };
		
	
	double Q[12*12] =
		{	1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
			0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
			0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
			0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
			0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
			0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
			0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0,
			0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0,
			0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0,
			0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0,
			0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0,
			0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0
		};
	
	double R[3*3] =
		{	1.0, 0.0, 0.0,
			0.0, 1.0, 0.0,
			0.0, 0.0, 1.0
		};
	double *S=0;
	
	double* P = Q;
	
	double A[12*12] =	/** problematric that only 4 digits precision?? */
// 		{	0.7627,    0.1149,    0.0025,    0.0000,    0.0000,    0.0000,    0.4596,    0.0198,    0.0003,    0.0000,    0.0000,    0.0000,
// 			0.1149,    0.7652,    0.1149,    0.0025,    0.0000,    0.0000,    0.0198,    0.4599,    0.0198,    0.0003,    0.0000,    0.0000,
// 			0.0025,    0.1149,    0.7652,    0.1149,    0.0025,    0.0000,    0.0003,    0.0198,    0.4599,    0.0198,    0.0003,    0.0000,
// 			0.0000,    0.0025,    0.1149,    0.7652,    0.1149,    0.0025,    0.0000,    0.0003,    0.0198,    0.4599,    0.0198,    0.0003,
// 			0.0000,    0.0000,    0.0025,    0.1149,    0.7652,    0.1149,    0.0000,    0.0000,    0.0003,    0.0198,    0.4599,    0.0198,
// 			0.0000,    0.0000,    0.0000,    0.0025,    0.1149,    0.7627,    0.0000,    0.0000,    0.0000,    0.0003,    0.0198,    0.4596,
// 			-0.8994,    0.4202,    0.0193,    0.0002,    0.0000,    0.0000,    0.7627,    0.1149,    0.0025,    0.0000,    0.0000,    0.0000,
// 			0.4202,   -0.8801,    0.4205,    0.0193,    0.0002,    0.0000,    0.1149,    0.7652,    0.1149,    0.0025,    0.0000,    0.0000,
// 			0.0193,    0.4205,   -0.8801,    0.4205,    0.0193,    0.0002,    0.0025,    0.1149,    0.7652,    0.1149,    0.0025,    0.0000,
// 			0.0002,    0.0193,    0.4205,   -0.8801,    0.4205,    0.0193,    0.0000,    0.0025,    0.1149,    0.7652,    0.1149,    0.0025,
// 			0.0000,    0.0002,    0.0193,    0.4205,   -0.8801,    0.4202,    0.0000,    0.0000,    0.0025,    0.1149,    0.7652,    0.1149,
// 			0.0000,    0.0000,    0.0002,    0.0193,    0.4202,   -0.8994,    0.0000,    0.0000,    0.0000,    0.0025,    0.1149,    0.7627
// 		};
		{	7.6272104759e-01,1.1488254659e-01,2.4765447407e-03,2.0938074941e-05,9.4222941754e-08,2.6306013529e-10,4.5961393973e-01,1.9813111713e-02,2.5126005603e-04,1.5075750676e-06,5.2612302474e-09,1.1999300634e-11,
		1.1488254659e-01,7.6519759233e-01,1.1490348467e-01,2.4766389636e-03,2.0938338001e-05,9.4222941754e-08,1.9813111713e-02,4.5986519978e-01,1.9814619288e-02,2.5126531726e-04,1.5075870669e-06,5.2612302474e-09,
		2.4765447407e-03,1.1490348467e-01,7.6519768656e-01,1.1490348493e-01,2.4766389636e-03,2.0938074941e-05,2.5126005603e-04,1.9814619288e-02,4.5986520504e-01,1.9814619300e-02,2.5126531726e-04,1.5075750676e-06,
		2.0938074941e-05,2.4766389636e-03,1.1490348493e-01,7.6519768656e-01,1.1490348467e-01,2.4765447407e-03,1.5075750676e-06,2.5126531726e-04,1.9814619300e-02,4.5986520504e-01,1.9814619288e-02,2.5126005603e-04,
		9.4222941754e-08,2.0938338001e-05,2.4766389636e-03,1.1490348467e-01,7.6519759233e-01,1.1488254659e-01,5.2612302474e-09,1.5075870669e-06,2.5126531726e-04,1.9814619288e-02,4.5986519978e-01,1.9813111713e-02,
		2.6306013529e-10,9.4222941754e-08,2.0938074941e-05,2.4765447407e-03,1.1488254659e-01,7.6272104759e-01,1.1999300634e-11,5.2612302474e-09,1.5075750676e-06,2.5126005603e-04,1.9813111713e-02,4.5961393973e-01,
		-8.9941476774e-01,4.2023897636e-01,1.9312099176e-02,2.4825016712e-04,1.4970646064e-06,5.2372316461e-09,7.6272104759e-01,1.1488254659e-01,2.4765447407e-03,2.0938074941e-05,9.4222941754e-08,2.6306013529e-10,
		4.2023897636e-01,-8.8010266857e-01,4.2048722652e-01,1.9313596240e-02,2.4825540436e-04,1.4970646064e-06,1.1488254659e-01,7.6519759233e-01,1.1490348467e-01,2.4766389636e-03,2.0938338001e-05,9.4222941754e-08,
		1.9312099176e-02,4.2048722652e-01,-8.8010117150e-01,4.2048723176e-01,1.9313596240e-02,2.4825016712e-04,2.4765447407e-03,1.1490348467e-01,7.6519768656e-01,1.1490348493e-01,2.4766389636e-03,2.0938074941e-05,
		2.4825016712e-04,1.9313596240e-02,4.2048723176e-01,-8.8010117150e-01,4.2048722652e-01,1.9312099176e-02,2.0938074941e-05,2.4766389636e-03,1.1490348493e-01,7.6519768656e-01,1.1490348467e-01,2.4765447407e-03,
		1.4970646064e-06,2.4825540436e-04,1.9313596240e-02,4.2048722652e-01,-8.8010266857e-01,4.2023897636e-01,9.4222941754e-08,2.0938338001e-05,2.4766389636e-03,1.1490348467e-01,7.6519759233e-01,1.1488254659e-01,
		5.2372316461e-09,1.4970646064e-06,2.4825016712e-04,1.9312099176e-02,4.2023897636e-01,-8.9941476774e-01,2.6306013529e-10,9.4222941754e-08,2.0938074941e-05,2.4765447407e-03,1.1488254659e-01,7.6272104759e-01
		};
	
	double B[12*3] =
// 		{	 0.1174,    0.0000,    0.0000,
// 			-0.1174,    0.0025,    0.0000,
// 			-0.0025,    0.1199,    0.0025,
// 			-0.0000,    0.0000,    0.1199,
// 			-0.0000,   -0.1199,    0.0000,
// 			-0.0000,   -0.0025,   -0.1199,
// 			0.4398,    0.0003,    0.0000,
// 			-0.4401,    0.0198,    0.0003,
// 			-0.0196,    0.4596,    0.0198,
// 			-0.0002,    0.0000,    0.4596,
// 			-0.0000,   -0.4596,    0.0000,
// 			-0.0000,   -0.0198,   -0.4594,
// 		};
		{
			1.1738012390e-01,2.1127047947e-05,9.4750064792e-08,
			-1.1740125121e-01,2.5187046136e-03,2.1127312010e-05,
			-2.4976720527e-03,1.1989882851e-01,2.5187046146e-03,
			-2.1032825507e-05,5.0104061525e-13,1.1989882877e-01,
			-9.4487004629e-08,-1.1989882825e-01,9.4750566331e-08,
			-2.6356150520e-10,-2.5186098636e-03,-1.1987770120e-01,
			4.3980082801e-01,2.5125479480e-04,1.5075630683e-06,
			-4.4005208807e-01,1.9813111701e-02,2.5126005603e-04,
			-1.9563359232e-02,4.5961393973e-01,1.9813111725e-02,
			-2.4975774219e-04,1.1999307797e-11,4.5961394499e-01,
			-1.5023258367e-06,-4.5961393447e-01,1.5075750676e-06,
			-5.2492309467e-09,-1.9811604138e-02,-4.5936267967e-01,			
		};
	
	double c[12] = 
		{	0.0,
			0.0,
			0.0,
			0.0,
			0.0,
			0.0,
			0.0,
			0.0,
			0.0,
			0.0,
			0.0,
			0.0	};
			
			
	double xLow[12] = 
		{	-4, -4, -4, -4, -4, -4, -4, -4, -4, -4, -4, -4		};
	
	double xUpp[12] = 
		{	4, 4, 4, 4, 4, 4, 4, 4, 4, 4, 4, 4,		};
	double uLow[3] = 
		{ -0.5, -0.5, -0.5 };
	double uUpp[3] = 
		{ 0.5, 0.5, 0.5 };
	/* double *xRef=0, *uRef=0; */
	
	qpData_t qpData;

	qpOptions_t qpOptions = qpDUNES_setupDefaultOptions();
	qpOptions.maxIter    = 20;
	qpOptions.printLevel = 1;
	qpOptions.stationarityTolerance = 1.e-6;
	qpOptions.equalityTolerance     = 2.221e-16;
	qpOptions.QPDUNES_ZERO             = 1.0e-50;
	qpOptions.QPDUNES_INFTY            = INFTY;
	qpOptions.maxNumLineSearchIterations            = 4;
	
	qpDUNES_setup( &qpData, nI, nX, nU, nD, &(qpOptions) );
	double t1 = getTime();
	return_t status;
	for ( k=0; k<1; ++k ) {
		for( i=0; i<nI; ++i )
		{
//		qpDUNES_setupSimpleBoundedInterval(  &qpData, qpData.intervals[i],Q,R,S, A,B,c, xLow,xUpp,uLow,uUpp );
		}
//	qpDUNES_setupSimpleBoundedInterval(  &qpData, qpData.intervals[nI], P,0,0, 0,0,0, xLow,xUpp,0,0 );

		status = qpDUNES_setupAllLocalQPs( &qpData, isLTI=QPDUNES_FALSE );	/* determine local QP solvers and set up auxiliary data */
		if (status != QPDUNES_OK) return 1;


// 	double t1 = getTime();
// 	for ( i=0; i<100; ++i ) {
//		qpDUNES_solve( &qpData, x0 );
		/* TODO: adapt to use MPC interface */
		status = qpDUNES_solve( &qpData );
		if (status != QPDUNES_OK) return 2;
	}
	double t2 = getTime();
	
// 	printf( "Computation time: %lf ms\n", 1e3*(t2-t1) );
	printf( "Average computation time of 100 runs: %lf ms\n", 1e3*(t2-t1)/1 );
	
	qpDUNES_cleanup( &qpData );
	
	return 0;
}


/*
 *	end of file
 */
